#ifndef MATRIX4_HPP
#define MATRIX4_HPP

#include "BasicMath.h"
#include "Vector3.hpp"
#include "Vector4.hpp"

class Matrix4
{
protected:
	double m[4][4];
public:
	Matrix4()
	{
		for (int i=0; i<4; ++i)
		{
			for (int j=0; j<4; ++j)
			{
				m[i][j] = 0;
			}
		}
	}
	Matrix4(
		double m00, double m01, double m02, double m03,
		double m10, double m11, double m12, double m13,
		double m20, double m21, double m22, double m23,
		double m30, double m31, double m32, double m33 )
	{
		m[0][0] = m00;
		m[0][1] = m01;
		m[0][2] = m02;
		m[0][3] = m03;
		m[1][0] = m10;
		m[1][1] = m11;
		m[1][2] = m12;
		m[1][3] = m13;
		m[2][0] = m20;
		m[2][1] = m21;
		m[2][2] = m22;
		m[2][3] = m23;
		m[3][0] = m30;
		m[3][1] = m31;
		m[3][2] = m32;
		m[3][3] = m33;
	}

	void set(
		double m00, double m01, double m02, double m03,
		double m10, double m11, double m12, double m13,
		double m20, double m21, double m22, double m23,
		double m30, double m31, double m32, double m33 )
	{
		m[0][0] = m00;
		m[0][1] = m01;
		m[0][2] = m02;
		m[0][3] = m03;
		m[1][0] = m10;
		m[1][1] = m11;
		m[1][2] = m12;
		m[1][3] = m13;
		m[2][0] = m20;
		m[2][1] = m21;
		m[2][2] = m22;
		m[2][3] = m23;
		m[3][0] = m30;
		m[3][1] = m31;
		m[3][2] = m32;
		m[3][3] = m33;
	}
	double get(int row, int column) const {
		return m[row][column];
	}
	Matrix4 mult(Matrix4 const &matrix4) const {
		Matrix4 myMatrix;
		int i = 0;
		int j = 0;
		int k = 0;
		for(i = 0; i < 4; i++) 
			for( j = 0; j < 4; j++)
				for( k = 0; k < 4; k++) 
					myMatrix.m[j][i] +=  m[k][i] * matrix4.m[j][k];
		/*std::cout << "first" << std::endl << " " << m[0][0] << " "<<
		m[0][1] << " "<<
		m[0][2] << " "<<
		m[0][3] << " "<< std::endl <<
		m[1][0]  << " "<<
		m[1][1]  << " "<<
		m[1][2]  << " "<<
		m[1][3]  << " "<< std::endl <<
		m[2][0]  << " "<<
		m[2][1]  << " "<<
		m[2][2]  << " "<<
		m[2][3]  << " "<< std::endl <<
		m[3][0]  << " "<<
		m[3][1] << " "<<
		m[3][2]  << " "<<
		m[3][3]  << " "<< std::endl;
		std::cout << "second"<< std::endl << matrix4.m[0][0] << " "<<
		matrix4.m[0][1] << " "<<
		matrix4.m[0][2] << " "<<
		matrix4.m[0][3] << " "<< std::endl <<
		matrix4.m[1][0]  << " "<<
		matrix4.m[1][1]  << " "<<
		matrix4.m[1][2]  << " "<<
		matrix4.m[1][3]  << " "<< std::endl <<
		matrix4.m[2][0]  << " "<<
		matrix4.m[2][1]  << " "<<
		matrix4.m[2][2]  << " "<<
		matrix4.m[2][3]  << " "<< std::endl <<
		matrix4.m[3][0]  << " "<<
		matrix4.m[3][1] << " "<<
		matrix4.m[3][2]  << " "<<
		matrix4.m[3][3]  << " "<< std::endl;
		std::cout << "third"<< std::endl <<  myMatrix.m[0][0] << " "<<
		myMatrix.m[0][1] << " "<<
		myMatrix.m[0][2] << " "<<
		myMatrix.m[0][3] << " "<< std::endl <<
		myMatrix.m[1][0]  << " "<<
		myMatrix.m[1][1]  << " "<<
		myMatrix.m[1][2]  << " "<<
		myMatrix.m[1][3]  << " "<< std::endl <<
		myMatrix.m[2][0]  << " "<<
		myMatrix.m[2][1]  << " "<<
		myMatrix.m[2][2]  << " "<<
		myMatrix.m[2][3]  << " "<< std::endl <<
		myMatrix.m[3][0]  << " "<<
		myMatrix.m[3][1] << " "<<
		myMatrix.m[3][2]  << " "<<
		myMatrix.m[3][3]  << " "<< std::endl;*/
		return myMatrix;

	}
	Vector3 mult3(Vector3 const &vector3) const {
		Vector4 myVector4 = Vector4(vector3[0],vector3[1],vector3[2], 1);
		Vector4 returnVector;
		double a[4] = {0,0,0,0};
		int i = 0;
		int j = 0;
		int k = 0;
		for(i = 0; i < 4; i++) 
				for( k = 0; k < 4; k++) 
					a[i] +=  m[k][i] * myVector4[k];
		returnVector.set(a[0],a[1],a[2],a[3]);
		returnVector = returnVector.dehomogenize();
		return Vector3(returnVector[0], returnVector[1], returnVector[2]);
	}
	Vector4 mult4(Vector4 const &vector4) const {
		Vector4 returnVector;
		double a[4] = {0,0,0,0};
		int i = 0;
		int j = 0;
		int k = 0;
		for(i = 0; i < 4; i++) 
				for( k = 0; k < 4; k++) 
					a[i] +=  m[k][i] * vector4[k];
		returnVector.set(a[0],a[1],a[2],a[3]);
		return returnVector;
	}

	Matrix4 transpose() {
		Matrix4 matrix;
		matrix.m[0][0] = m[0][0];
		matrix.m[0][1] = m[1][0];
		matrix.m[0][2] = m[2][0];
		matrix.m[0][3] = m[3][0];
		matrix.m[1][0] = m[0][1];
		matrix.m[1][1] = m[1][1];
		matrix.m[1][2] = m[2][1];
		matrix.m[1][3] = m[3][1];
		matrix.m[2][0] = m[0][2];
		matrix.m[2][1] = m[1][2];
		matrix.m[2][2] = m[2][2];
		matrix.m[2][3] = m[3][2];
		matrix.m[3][0] = m[0][3];
		matrix.m[3][1] = m[1][3];
		matrix.m[3][2] = m[2][3];
		matrix.m[3][3] = m[3][3];
		return matrix;
	}
	// void dump() {
	// 	std::cout << 
	// 	m[0][0] << " "<<
	// 	m[0][1] << " "<<
	// 	m[0][2] << " "<<
	// 	m[0][3] << " "<< std::endl <<
	// 	m[1][0] << " "<<
	// 	m[1][1] << " "<<
	// 	m[1][2] << " "<<
	// 	m[1][3] << " "<< std::endl <<
	// 	m[2][0] << " "<<
	// 	m[2][1] << " "<<
	// 	m[2][2] << " "<<
	// 	m[2][3] << " "<< std::endl <<
	// 	m[3][0] << " "<<
	// 	m[3][1] << " "<<
	// 	m[3][2] << " "<<
	// 	m[3][3] << " "<< std::endl;
	// }
		// angle in degrees
	void rotateX(double angle)
	{
		double th = BasicMath::radian(angle);
		m[0][0] = 1;
		m[0][1] = 0;
		m[0][2] = 0;
		m[0][3] = 0;
		m[1][0] = 0;
		m[1][1] = cos(th);
		m[1][2] = sin(th);
		m[1][3] = 0;
		m[2][0] = 0;
		m[2][1] = -sin(th);
		m[2][2] = cos(th);
		m[2][3] = 0;
		m[3][0] = 0;
		m[3][1] = 0;
		m[3][2] = 0;
		m[3][3] = 1;
	}
		// angle in degrees
	void rotateY(double angle)
	{
		double th = BasicMath::radian(angle);
		m[0][0] = cos(th);
		m[0][1] = 0;
		m[0][2] = -sin(th);
		m[0][3] = 0;
		m[1][0] = 0;
		m[1][1] = 1;
		m[1][2] = 0;
		m[1][3] = 0;
		m[2][0] = sin(th);
		m[2][1] = 0;
		m[2][2] = cos(th);
		m[2][3] = 0;
		m[3][0] = 0;
		m[3][1] = 0;
		m[3][2] = 0;
		m[3][3] = 1;
	}
	// angle in degrees
	void rotateZ(double angle)
	{
		double th = BasicMath::radian(angle);
		m[0][0] = cos(th);
		m[0][1] = sin(th);
		m[0][2] = 0;
		m[0][3] = 0;
		m[1][0] = -sin(th);
		m[1][1] = cos(th);
		m[1][2] = 0;
		m[1][3] = 0;
		m[2][0] = 0;
		m[2][1] = 0;
		m[2][2] = 1;
		m[2][3] = 0;
		m[3][0] = 0;
		m[3][1] = 0;
		m[3][2] = 0;
		m[3][3] = 1;
	}

	//must be unit vector
	void rotateArbitrary(Vector3 a, double angle) {
		double th = BasicMath::radian(angle);
		a = a.normalize();
		m[0][0] = 1+(1-cos(th))*(a[0]*a[0]-1);
		m[0][1] = a[2]*sin(th)+(1-cos(th))*a[1]*a[0];
		m[0][2] = -a[1]*sin(th)+(1-cos(th))*a[2]*a[0];
		m[0][3] = 0;
		m[1][0] = -a[2]*sin(th)+(1-cos(th))*a[0]*a[1];
		m[1][1] = 1+(1-cos(th))*(a[1]*a[1]-1);
		m[1][2] = a[0]*sin(th)+(1-cos(th))*a[2]*a[1];
		m[1][3] = 0;
		m[2][0] = a[1]*sin(th)+(1-cos(th))*a[0]*a[2];
		m[2][1] = -a[0]*sin(th)+(1-cos(th))*a[1]*a[2];
		m[2][2] = 1+(1-cos(th))*(a[2]*a[2]-1);
		m[2][3] = 0;
		m[3][0] = 0;
		m[3][1] = 0;
		m[3][2] = 0;
		m[3][3] = 1;

		//std::cout << "x: " << a[0] << " y: " << a[1] << " z: " << a[2] << std::endl;
		//std::cout << "angle: " << th << std::endl;
		//std::cout << " "<< m[0][0] << " "<<
		//m[0][1] << " "<<
		//m[0][2] << " "<<
		//m[0][3] << " "<< std::endl <<
		//m[1][0]  << " "<<
		//m[1][1]  << " "<<
		//m[1][2]  << " "<<
		//m[1][3]  << " "<< std::endl <<
		//m[2][0]  << " "<<
		//m[2][1]  << " "<<
		//m[2][2]  << " "<<
		//m[2][3]  << " "<< std::endl <<
		//m[3][0]  << " "<<
		//m[3][1] << " "<<
		//m[3][2]  << " "<<
		//m[3][3]  << " "<< std::endl;
	}
	void scale(double x, double y, double z) {
		m[0][0] = x;
		m[0][1] = 0;
		m[0][2] = 0;
		m[0][3] = 0;
		m[1][0] = 0;
		m[1][1] = y;
		m[1][2] = 0;
		m[1][3] = 0;
		m[2][0] = 0;
		m[2][1] = 0;
		m[2][2] = z;
		m[2][3] = 0;
		m[3][0] = 0;
		m[3][1] = 0;
		m[3][2] = 0;
		m[3][3] = 1;
	}
	void translate(double x, double y, double z) {
		m[0][0] = 1;
		m[0][1] = 0;
		m[0][2] = 0;
		m[0][3] = 0;
		m[1][0] = 0;
		m[1][1] = 1;
		m[1][2] = 0;
		m[1][3] = 0;
		m[2][0] = 0;
		m[2][1] = 0;
		m[2][2] = 1;
		m[2][3] = 0;
		m[3][0] = x;
		m[3][1] = y;
		m[3][2] = z;
		m[3][3] = 1;
				
		//m[0][0] = 1;
		//m[0][1] = 0;
		//m[0][2] = 0;
		//m[0][3] = x;
		//m[1][0] = 0;
		//m[1][1] = 1;
		//m[1][2] = 0;
		//m[1][3] = y;
		//m[2][0] = 0;
		//m[2][1] = 0;
		//m[2][2] = 1;
		//m[2][3] = z;
		//m[3][0] = 0;
		//m[3][1] = 0;
		//m[3][2] = 0;
		//m[3][3] = 1;
	}

	double* getPointer()
	{
		return &m[0][0];
	}

	void identity()
	{
		double ident[4][4]={{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}};
		for (int i=0; i<4; ++i)
		{
			for (int j=0; j<4; ++j)
			{
				m[i][j] = ident[i][j];
			}
		}
	}
};
#endif